
#include "motion_control.h"
#include <chrono>
#include <memory>
#include <errno.h>
#include <fcntl.h> 
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <termios.h>
#include <unistd.h>
#include <iostream>
#include <time.h>
#include <math.h>

MotionControl::MotionControl()
{
}

void MotionControl::SetController(char pretype)
{
	usvtype = pretype;
	switch(usvtype){
	case 2:
		headingcontroller = PIDControl(0.35, 0, 3, 2000, -30, 30);
		velocitycontroller = PIDControl(200, 1, 0, 2000, 10, 1000);
		break;
	default:
		headingcontroller = PIDControl(0.35, 0, 3, 2000, -30, 30);
		velocitycontroller = PIDControl(200, 1, 0, 2000, 10, 1000);
		break;
	}
}

actuator_control MotionControl::USVControl(headvel_control target, TransState boatstate)
{
	actuator_control preaction;
	memset(&preaction,0,sizeof(actuator_control));
	float phid = target.heading;
	float ud = target.velocity;
	float vx = boatstate.VelEarth[0];
	float vy = boatstate.VelEarth[1];
	float phi = boatstate.VelEarth[3];
	
	float w = sqrtf(vx*vx + vy*vy);
	float errphi = phid - phi;
	if(errphi > 180) errphi -= 360;
	if(errphi < -180) errphi += 360;
	float abserrphi = fabs(errphi);
	if(abserrphi > 30){
		ud = ud*((180 - abserrphi)/180);  //转弯速度衰减
		if(ud<(0.2*ud)) ud = 0.2*ud;
	}
	float steer = headingcontroller.PID_Control(errphi);


	float speed = 0;
	float erru = 0;
	if(target.model == 2){
		erru = ud - w;
		speed = velocitycontroller.PID_Control(erru);
		if(ud == 0)  speed = 0;
	}else if((target.model == 0)||(target.model == 1)){
		speed = ud;
	}

	preaction.steer_l = steer;
	preaction.motor_l = speed;
	return preaction;
}
